# coding=utf-8
# 2019/12/12: 首个记录
# 2019/12/25: 添加若干标签种类
# 2020/4/1: 导入bi_common。修正cont变量初始化问题
# 2020/5/27: 支持协议v6
# 2020/6/17: 修正目标物heading插值问题

import bi_common as bi
from math import pi, sin, cos, atan2, sqrt


# 位置数据模式
class PositionMode:
    CLOSEST_POINT = 1  # 最近点
    BOX_CENTER = 2  # 框中心

    def __init__(self):
        pass


# 目标物类别
class ObjectClass:
    GENERAL = 1  # 一般物体大类
    CAR = 2  # 车辆大类
    PED = 3  # 行人大类
    TRUCK = 4  # 货车大类
    BIKE = 5  # Bike大类
    RAIL_CAR = 6  # 轨道车辆大类
    SPECIAL = 7  # 特殊物体大类
    ROAD_STATIC = 8  # 道路内静态物体大类
    SIDE_STATIC = 9  # 道路外静态物体大类

    GENERAL_SMALL = 11  # 一般小物体
    GENERAL_BIG = 12  # 一般大物体
    BARRIER = 13  # 一般障碍物

    VAN = 21  # 面包车
    MINIBUS = 22  # 小巴
    BUS = 23  # 大巴
    BATTERY_CART = 24  # 园区电瓶车
    TINY_CAR = 25  # 微型车
    SUV = 26  # SUV

    ADULT = 31  # 成人
    CHILD = 32  # 小孩
    SCOOTER = 33  # 平衡车
    WHEEL_CHAIR = 34  # 轮椅

    MINITRUCK = 41  # 小卡车
    CONTAINER_TRUCK = 42  # 货柜车
    SPECIAL_CAR = 43  # 特种车辆
    TRAILER = 44  # 拖车

    MOTORBIKE = 51  # 摩托车
    BICYCLE = 52  # 自行车
    ELECTRIC_BIKE = 53  # 电瓶自行车
    TRICYCLE = 54  # 三轮车

    TRAIN = 61  # 火车
    TRAM = 62  # 有轨电车

    ANIMAL = 71  # 动物
    BALL = 72  # 球类
    LITTER = 73  # 垃圾等杂物

    CONE = 81  # 锥形路障
    MANHOLE_COVER = 82  # 井盖
    PATCH = 83  # 路面补丁
    GANTRY = 84  # 龙门架

    POLE = 91  # 竖杆
    TREE = 92  # 树木
    VEGETATION = 93  # 灌木
    BUILDING = 94  # 建筑物

    def __init__(self):
        pass


# 目标物颜色
class ObjectColor:
    def __init__(self):
        self.valid = False  # 颜色是否有效
        self.r = 0  # 目标物的颜色R分量
        self.g = 0  # 目标物的颜色G分量
        self.b = 0  # 目标物的颜色B分量


# 目标物信息
class ObjectInfo:
    def __init__(self):
        self.id = 0  # 目标物ID
        self.age = 0  # 目标物的Age
        self.raw_id_valid = False
        self.raw_id = 0  # 目标物的原始ID
        self.raw_age_valid = False
        self.raw_age = 0  # 目标物的原始Age
        self.confidence_valid = False
        self.confidence = 0.0  # 目标物的置信度 %
        self.time_offset_valid = False
        self.time_offset = 0  # 目标物的时间偏置 us
        self.color = ObjectColor()  # 目标物的颜色
        self.classification = ObjectClass.GENERAL  # 目标物的类别
        self.class_confidence_valid = False
        self.class_confidence = 0.0  # 目标物的分类置信度
        self.raw_class_id_valid = False
        self.raw_class_id = 0  # 目标物的原始类别ID
        self.pos_mode = PositionMode.CLOSEST_POINT  # 目标物的位置模式
        self.posx = 0.0  # 目标物的x轴方向位置 m
        self.posy = 0.0  # 目标物的y轴方向位置 m
        self.posz = 0.0  # 目标物的z轴方向位置 m
        self.posx_sigma_valid = False
        self.posx_sigma = 0.0  # 目标物x轴方向位置的精度 m
        self.posy_sigma_valid = False
        self.posy_sigma = 0.0  # 目标物y轴方向位置的精度 m
        self.posz_sigma_valid = False
        self.posz_sigma = 0.0  # 目标物z轴方向位置的精度 m
        self.cpx = 0.0  # 目标物的最近点x轴坐标 m
        self.cpy = 0.0  # 目标物的最近点y轴坐标 m
        self.cpd = 0.0  # 目标物的最近点与本车轮廓距离 m
        self.width_valid = False
        self.width = 0.0  # 目标物的宽度 m
        self.length_valid = False
        self.length = 0.0  # 目标物的长度 m
        self.height_valid = False
        self.height = 0.0  # 目标物的高度 m
        self.heading_valid = False
        self.heading = 0.0  # 目标物的朝向 deg
        self.vx_rel_valid = False
        self.vx_rel = 0.0  # x轴方向相对速度 KPH
        self.vx_abs_valid = False
        self.vx_abs = 0.0  # x轴方向绝对速度 KPH
        self.vy_rel_valid = False
        self.vy_rel = 0.0  # y轴方向相对速度 KPH
        self.vy_abs_valid = False
        self.vy_abs = 0.0  # y轴方向绝对速度 KPH
        self.ax_rel_valid = False
        self.ax_rel = 0.0  # x轴方向相对加速度 m/s²
        self.ax_abs_valid = False
        self.ax_abs = 0.0  # x轴方向绝对加速度 m/s²
        self.ay_rel_valid = False
        self.ay_rel = 0.0  # y轴方向相对加速度 m/s²
        self.ay_abs_valid = False
        self.ay_abs = 0.0  # y轴方向绝对加速度 m/s²
        self.yaw_rate_valid = False
        self.yaw_rate = 0.0  # 横摆角速度 deg/s
        self.curvature_valid = False
        self.curvature = 0.0  # 转弯曲率 1/m
        self.contour = []  # 目标物轨迹点的列表，按x1,y1,x2,y2...排列

    def category(self):
        if self.classification < 10:
            return self.classification
        else:
            return int(self.classification / 10)

    def speed(self):
        if self.vx_abs_valid and self.vy_abs_valid:
            return sqrt(self.vx_abs * self.vx_abs + self.vy_abs * self.vy_abs)
        else:
            return None


# 传感器FOV
class ObjectSensorFov:
    def __init__(self):
        self.position_x = 0.0  # FOV中心点x轴坐标 m
        self.position_y = 0.0  # FOV中心的y轴坐标 m
        self.angle_range = 90.0  # FOV的角度范围 deg
        self.orientation = 0.0  # FOV中轴线朝向角 deg
        self.distance_range = 100.0  # FOV探测距离范围 m
        self.blind_range = 0.0  # FOV盲区范围 m


# 目标物传感器样本
class ObjectSensorSample:
    def __init__(self):
        self.time = 0.0  # 时间戳 s
        self.objects = []  # 目标物列表
        self.cipv_index = -1  # 前向关键目标序号，-1表示不存在
        self.lko_index = -1  # 左侧关键目标序号，-1表示不存在
        self.rko_index = -1  # 右侧关键目标序号，-1表示不存在
        self.fovs = []  # FOV列表

    # 转通用样本，用于样本输出
    def to_general_sample(self, channel):
        output = bi.agency.create_general_sample()
        output.protocol = "obj-sensor-sample-v6@" + str(channel)
        output.time = self.time
        contour_points = 0
        contour_offsets = []
        contour_sizes = []
        for obj in self.objects:
            point_count = len(obj.contour) / 2
            contour_offsets.append(contour_points)
            contour_sizes.append(point_count)
            contour_points += point_count
        output.significant = 16 + len(self.objects) * 52 + len(self.fovs) * 6 + contour_points * 2
        output.values = []
        output.values.append(len(self.objects))
        output.values.append(len(self.fovs))
        output.values.append(0)
        output.values.append(contour_points)
        output.values.append(self.cipv_index if self.cipv_index >= 0 else None)
        output.values.append(self.lko_index if self.lko_index >= 0 else None)
        output.values.append(self.rko_index if self.rko_index >= 0 else None)
        output.values.append(None)
        output.values.append(None)
        output.values.append(None)
        output.values.append(None)
        output.values.append(None)
        output.values.append(None)
        output.values.append(None)
        output.values.append(None)
        output.values.append(None)
        i = 0
        for obj in self.objects:
            output.values.append(obj.id)
            output.values.append(obj.age)
            output.values.append(obj.raw_id if obj.raw_id_valid else None)
            output.values.append(obj.raw_age if obj.raw_age_valid else None)
            output.values.append(obj.raw_class_id if obj.raw_class_id_valid else None)
            output.values.append(obj.classification)
            output.values.append(obj.pos_mode)
            output.values.append(obj.posx)
            output.values.append(obj.posy)
            output.values.append(obj.cpx)
            output.values.append(obj.cpy)
            output.values.append(obj.cpd)
            output.values.append(obj.width if obj.width_valid else None)
            output.values.append(obj.length if obj.length_valid else None)
            output.values.append(obj.heading if obj.heading_valid else None)
            output.values.append(obj.vx_rel if obj.vx_rel_valid else None)
            output.values.append(obj.vx_abs if obj.vx_abs_valid else None)
            output.values.append(obj.vy_rel if obj.vy_rel_valid else None)
            output.values.append(obj.vy_abs if obj.vy_abs_valid else None)
            output.values.append(obj.ax_rel if obj.ax_rel_valid else None)
            output.values.append(obj.ax_abs if obj.ax_abs_valid else None)
            output.values.append(obj.ay_rel if obj.ay_rel_valid else None)
            output.values.append(obj.ay_abs if obj.ay_abs_valid else None)
            output.values.append(obj.color.r if obj.color.valid else None)
            output.values.append(obj.color.g if obj.color.valid else None)
            output.values.append(obj.color.b if obj.color.valid else None)
            output.values.append(obj.time_offset if obj.time_offset_valid else None)
            output.values.append(obj.confidence if obj.confidence_valid else None)
            output.values.append(obj.class_confidence if obj.class_confidence_valid else None)
            output.values.append(obj.height if obj.height_valid else None)
            output.values.append(obj.posz)
            output.values.append(obj.posx_sigma if obj.posx_sigma_valid else None)
            output.values.append(obj.posy_sigma if obj.posy_sigma_valid else None)
            output.values.append(obj.posz_sigma if obj.posz_sigma_valid else None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(obj.yaw_rate if obj.yaw_rate_valid else None)
            output.values.append(obj.curvature if obj.curvature_valid else None)
            output.values.append(obj.speed())
            output.values.append(0)
            output.values.append(0)
            output.values.append(contour_offsets[i])
            output.values.append(contour_sizes[i])
            i += 1
        for fov in self.fovs:
            output.values.append(fov.position_x)
            output.values.append(fov.position_y)
            output.values.append(fov.orientation)
            output.values.append(fov.angle_range)
            output.values.append(fov.distance_range)
            output.values.append(fov.blind_range)
        for obj in self.objects:
            output.values += obj.contour
        return output


def _conv_obj_sensor_sample_v5(gs):
    values_count = len(gs.values)
    if values_count < 11:
        return None
    object_count = int(gs.values[0]) if gs.values[0] is not None else 0
    trajectory_count = int(gs.values[1]) if gs.values[1] is not None else 0
    contour_count = int(gs.values[2]) if gs.values[2] is not None else 0
    fov_count = int(gs.values[10]) if gs.values[10] is not None else 0
    size_with_extra = 11 + object_count * 42 + fov_count * 6 + (trajectory_count + contour_count) * 2
    size_without_extra = 11 + object_count * 42 + fov_count * 6
    if values_count != size_with_extra and values_count != size_without_extra:
        return None
    output = ObjectSensorSample()
    output.time = gs.time
    output.cipv_index = int(gs.values[3]) if gs.values[3] is not None else -1
    output.lko_index = int(gs.values[4]) if gs.values[4] is not None else -1
    output.rko_index = int(gs.values[5]) if gs.values[5] is not None else -1
    contour_base = 11 + object_count * 42 + fov_count * 6 + trajectory_count * 2
    for i in range(0, object_count):
        obj = ObjectInfo()
        b = 11 + 42 * i
        obj.id = int(gs.values[b] if gs.values[b] is not None else 0)
        obj.age = int(gs.values[b + 1] if gs.values[b + 1] is not None else 0)
        if gs.values[b + 2] is not None:
            obj.raw_id_valid = True
            obj.raw_id = int(gs.values[b + 2])
        if gs.values[b + 3] is not None:
            obj.raw_age_valid = True
            obj.raw_age = int(gs.values[b + 3])
        if gs.values[b + 4] is not None:
            obj.raw_class_id_valid = True
            obj.raw_class_id = int(gs.values[b + 4])
        obj.classification = int(gs.values[b + 5]) if gs.values[b + 5] is not None else ObjectClass.GENERAL
        obj.pos_mode = int(gs.values[b + 6]) if gs.values[b + 6] is not None else PositionMode.CLOSEST_POINT
        obj.posx = float(gs.values[b + 7]) if gs.values[b + 7] is not None else 0.0
        obj.posy = float(gs.values[b + 8]) if gs.values[b + 8] is not None else 0.0
        obj.cpx = float(gs.values[b + 9]) if gs.values[b + 9] is not None else 0.0
        obj.cpy = float(gs.values[b + 10]) if gs.values[b + 10] is not None else 0.0
        obj.cpd = float(gs.values[b + 11]) if gs.values[b + 11] is not None else 0.0
        if gs.values[b + 12] is not None:
            obj.width_valid = True
            obj.width = float(gs.values[b + 12])
        if gs.values[b + 13] is not None:
            obj.length_valid = True
            obj.length = float(gs.values[b + 13])
        if gs.values[b + 14] is not None:
            obj.heading_valid = True
            obj.heading = float(gs.values[b + 14])
        if gs.values[b + 15] is not None:
            obj.vx_rel_valid = True
            obj.vx_rel = float(gs.values[b + 15])
        if gs.values[b + 16] is not None:
            obj.vx_abs_valid = True
            obj.vx_abs = float(gs.values[b + 16])
        if gs.values[b + 17] is not None:
            obj.vy_rel_valid = True
            obj.vy_rel = float(gs.values[b + 17])
        if gs.values[b + 18] is not None:
            obj.vy_abs_valid = True
            obj.vy_abs = float(gs.values[b + 18])
        if gs.values[b + 19] is not None:
            obj.ax_rel_valid = True
            obj.ax_rel = float(gs.values[b + 19])
        if gs.values[b + 20] is not None:
            obj.ax_abs_valid = True
            obj.ax_abs = float(gs.values[b + 20])
        if gs.values[b + 21] is not None:
            obj.ay_rel_valid = True
            obj.ay_rel = float(gs.values[b + 21])
        if gs.values[b + 22] is not None:
            obj.ay_abs_valid = True
            obj.ay_abs = float(gs.values[b + 22])
        cont_ok = False
        cont_offset = 0
        cont_size = 0
        if gs.values[b + 33] is not None and gs.values[b + 34] is not None:
            cont_offset = int(gs.values[b + 33])
            cont_size = int(gs.values[b + 34])
            cont_ok = True
        if gs.values[b + 35] is not None:
            obj.time_offset_valid = True
            obj.time_offset = int(gs.values[b + 35])
        if gs.values[b + 36] is not None and gs.values[b + 37] is not None and gs.values[b + 38] is not None:
            obj.color.valid = True
            obj.color.r = int(gs.values[b + 36])
            obj.color.g = int(gs.values[b + 37])
            obj.color.b = int(gs.values[b + 38])
        if gs.values[b + 39] is not None:
            obj.class_confidence_valid = True
            obj.class_confidence = float(gs.values[b + 39])
        if gs.values[b + 40] is not None:
            obj.posx_sigma_valid = True
            obj.posx_sigma = float(gs.values[b + 40])
        if gs.values[b + 41] is not None:
            obj.posy_sigma_valid = True
            obj.posy_sigma = float(gs.values[b + 41])
        if values_count == size_with_extra and cont_ok:
            obj.contour = gs.values[(contour_base + cont_offset * 2):(contour_base + (cont_offset + cont_size) * 2)]
        output.objects.append(obj)
    return output


def _conv_obj_sensor_sample_v6(gs):
    values_count = len(gs.values)
    if values_count < 16:
        return None
    object_count = int(gs.values[0]) if gs.values[0] is not None else 0
    fov_count = int(gs.values[1]) if gs.values[1] is not None else 0
    trajectory_count = int(gs.values[2]) if gs.values[2] is not None else 0
    contour_count = int(gs.values[3]) if gs.values[3] is not None else 0
    size_with_extra = 16 + object_count * 52 + fov_count * 6 + (trajectory_count + contour_count) * 2
    size_without_extra = 16 + object_count * 52 + fov_count * 6
    if values_count != size_with_extra and values_count != size_without_extra:
        return None
    output = ObjectSensorSample()
    output.time = gs.time
    output.cipv_index = int(gs.values[4]) if gs.values[4] is not None else -1
    output.lko_index = int(gs.values[5]) if gs.values[5] is not None else -1
    output.rko_index = int(gs.values[6]) if gs.values[6] is not None else -1
    contour_base = 16 + object_count * 52 + fov_count * 6 + trajectory_count * 2
    for i in range(0, object_count):
        obj = ObjectInfo()
        b = 16 + 52 * i
        obj.id = int(gs.values[b] if gs.values[b] is not None else 0)
        obj.age = int(gs.values[b + 1] if gs.values[b + 1] is not None else 0)
        if gs.values[b + 2] is not None:
            obj.raw_id_valid = True
            obj.raw_id = int(gs.values[b + 2])
        if gs.values[b + 3] is not None:
            obj.raw_age_valid = True
            obj.raw_age = int(gs.values[b + 3])
        if gs.values[b + 4] is not None:
            obj.raw_class_id_valid = True
            obj.raw_class_id = int(gs.values[b + 4])
        obj.classification = int(gs.values[b + 5]) if gs.values[b + 5] is not None else ObjectClass.GENERAL
        obj.pos_mode = int(gs.values[b + 6]) if gs.values[b + 6] is not None else PositionMode.CLOSEST_POINT
        obj.posx = float(gs.values[b + 7]) if gs.values[b + 7] is not None else 0.0
        obj.posy = float(gs.values[b + 8]) if gs.values[b + 8] is not None else 0.0
        obj.cpx = float(gs.values[b + 9]) if gs.values[b + 9] is not None else 0.0
        obj.cpy = float(gs.values[b + 10]) if gs.values[b + 10] is not None else 0.0
        obj.cpd = float(gs.values[b + 11]) if gs.values[b + 11] is not None else 0.0
        if gs.values[b + 12] is not None:
            obj.width_valid = True
            obj.width = float(gs.values[b + 12])
        if gs.values[b + 13] is not None:
            obj.length_valid = True
            obj.length = float(gs.values[b + 13])
        if gs.values[b + 14] is not None:
            obj.heading_valid = True
            obj.heading = float(gs.values[b + 14])
        if gs.values[b + 15] is not None:
            obj.vx_rel_valid = True
            obj.vx_rel = float(gs.values[b + 15])
        if gs.values[b + 16] is not None:
            obj.vx_abs_valid = True
            obj.vx_abs = float(gs.values[b + 16])
        if gs.values[b + 17] is not None:
            obj.vy_rel_valid = True
            obj.vy_rel = float(gs.values[b + 17])
        if gs.values[b + 18] is not None:
            obj.vy_abs_valid = True
            obj.vy_abs = float(gs.values[b + 18])
        if gs.values[b + 19] is not None:
            obj.ax_rel_valid = True
            obj.ax_rel = float(gs.values[b + 19])
        if gs.values[b + 20] is not None:
            obj.ax_abs_valid = True
            obj.ax_abs = float(gs.values[b + 20])
        if gs.values[b + 21] is not None:
            obj.ay_rel_valid = True
            obj.ay_rel = float(gs.values[b + 21])
        if gs.values[b + 22] is not None:
            obj.ay_abs_valid = True
            obj.ay_abs = float(gs.values[b + 22])
        if gs.values[b + 23] is not None and gs.values[b + 24] is not None and gs.values[b + 25] is not None:
            obj.color.valid = True
            obj.color.r = int(gs.values[b + 23])
            obj.color.g = int(gs.values[b + 24])
            obj.color.b = int(gs.values[b + 25])
        if gs.values[b + 26] is not None:
            obj.time_offset_valid = True
            obj.time_offset = int(gs.values[b + 26])
        if gs.values[b + 27] is not None:
            obj.confidence_valid = True
            obj.confidence = float(gs.values[b + 27])
        if gs.values[b + 28] is not None:
            obj.class_confidence_valid = True
            obj.class_confidence = float(gs.values[b + 28])
        if gs.values[b + 29] is not None:
            obj.height_valid = True
            obj.height = float(gs.values[b + 29])
        obj.posz = float(gs.values[b + 30]) if gs.values[b + 30] is not None else 0.0
        if gs.values[b + 31] is not None:
            obj.posx_sigma_valid = True
            obj.posx_sigma = float(gs.values[b + 31])
        if gs.values[b + 32] is not None:
            obj.posy_sigma_valid = True
            obj.posy_sigma = float(gs.values[b + 32])
        if gs.values[b + 33] is not None:
            obj.posz_sigma_valid = True
            obj.posz_sigma = float(gs.values[b + 33])
        if gs.values[b + 45] is not None:
            obj.yaw_rate_valid = True
            obj.yaw_rate = float(gs.values[b + 45])
        if gs.values[b + 46] is not None:
            obj.curvature_valid = True
            obj.curvature = float(gs.values[b + 46])
        cont_ok = False
        cont_offset = 0
        cont_size = 0
        if gs.values[b + 50] is not None and gs.values[b + 51] is not None:
            cont_offset = int(gs.values[b + 50])
            cont_size = int(gs.values[b + 51])
            cont_ok = True
        if values_count == size_with_extra and cont_ok:
            obj.contour = gs.values[(contour_base + cont_offset * 2):(contour_base + (cont_offset + cont_size) * 2)]
        output.objects.append(obj)
    return output


def _interpolate_angle(a1, w1, a2, w2):
    deg2rad = pi / 180
    x1 = cos(a1 * deg2rad)
    y1 = sin(a1 * deg2rad)
    x2 = cos(a2 * deg2rad)
    y2 = sin(a2 * deg2rad)
    xo = x1 * w1 + x2 * w2
    yo = y1 * w1 + y2 * w2
    if xo == 0 and yo == 0:
        return None
    return atan2(yo, xo) / deg2rad


def _interpolate_obj_sensor_sample(s1, w1, s2, w2):
    output = ObjectSensorSample()
    output.time = bi.time
    output.fovs = s1.fovs
    cipv_id = -1
    if s1.cipv_index >= 0 and s2.cipv_index >= 0 and s1.objects[s1.cipv_index].id == s2.objects[s2.cipv_index].id:
        cipv_id = s1.objects[s1.cipv_index].id
    lko_id = -1
    if s1.lko_index >= 0 and s2.lko_index >= 0 and s1.objects[s1.lko_index].id == s2.objects[s2.lko_index].id:
        lko_id = s1.objects[s1.lko_index].id
    rko_id = -1
    if s1.rko_index >= 0 and s2.rko_index >= 0 and s1.objects[s1.rko_index].id == s2.objects[s2.rko_index].id:
        rko_id = s1.objects[s1.rko_index].id
    index = 0
    for o1 in s1.objects:
        id = o1.id
        for o2 in s2.objects:
            if o2.id != id:
                continue
            if cipv_id == id:
                output.cipv_index = index
            if lko_id == id:
                output.lko_index = index
            if rko_id == id:
                output.rko_index = index
            obj = ObjectInfo()
            obj.id = id
            obj.age = o1.age
            obj.raw_id_valid = o1.raw_id_valid
            obj.raw_id = o1.raw_id
            obj.raw_age_valid = o1.raw_age_valid
            obj.raw_age = o1.raw_age
            obj.confidence_valid = o1.confidence_valid if w1 > w2 else o2.confidence_valid
            obj.confidence = o1.confidence if w1 > w2 else o2.confidence
            if o1.time_offset_valid and o2.time_offset_valid:
                obj.time_offset_valid = True
                obj.time_offset = int(o1.time_offset * w1 + o2.time_offset * w2)
            obj.color = o1.color if w1 > w2 else o2.color
            obj.classification = o1.classification if w1 > w2 else o2.classification
            obj.raw_class_id_valid = o1.raw_class_id_valid if w1 > w2 else o2.raw_class_id_valid
            obj.raw_class_id = o1.raw_class_id if w1 > w2 else o2.raw_class_id
            obj.class_confidence_valid = o1.class_confidence_valid if w1 > w2 else o2.class_confidence_valid
            obj.class_confidence = o1.class_confidence if w1 > w2 else o2.class_confidence
            obj.pos_mode = o1.pos_mode
            obj.posx = o1.posx * w1 + o2.posx * w2
            obj.posy = o1.posy * w1 + o2.posy * w2
            obj.posz = o1.posz * w1 + o2.posz * w2
            if o1.posx_sigma_valid and o2.posx_sigma_valid:
                obj.posx_sigma_valid = True
                obj.posx_sigma = o1.posx_sigma * w1 + o2.posx_sigma * w2
            if o1.posy_sigma_valid and o2.posy_sigma_valid:
                obj.posy_sigma_valid = True
                obj.posy_sigma = o1.posy_sigma * w1 + o2.posy_sigma * w2
            if o1.posz_sigma_valid and o2.posz_sigma_valid:
                obj.posz_sigma_valid = True
                obj.posz_sigma = o1.posz_sigma * w1 + o2.posz_sigma * w2
            obj.cpx = o1.cpx * w1 + o2.cpx * w2
            obj.cpy = o1.cpy * w1 + o2.cpy * w2
            obj.cpd = o1.cpd * w1 + o2.cpd * w2
            if o1.vx_abs_valid and o2.vx_abs_valid:
                obj.vx_abs_valid = True
                obj.vx_abs = o1.vx_abs * w1 + o2.vx_abs * w2
            if o1.vy_abs_valid and o2.vy_abs_valid:
                obj.vy_abs_valid = True
                obj.vy_abs = o1.vy_abs * w1 + o2.vy_abs * w2
            if o1.vx_rel_valid and o2.vx_rel_valid:
                obj.vx_rel_valid = True
                obj.vx_rel = o1.vx_rel * w1 + o2.vx_rel * w2
            if o1.vy_rel_valid and o2.vy_rel_valid:
                obj.vy_rel_valid = True
                obj.vy_rel = o1.vy_rel * w1 + o2.vy_rel * w2
            if o1.ax_abs_valid and o2.ax_abs_valid:
                obj.ax_abs_valid = True
                obj.ax_abs = o1.ax_abs * w1 + o2.ax_abs * w2
            if o1.ay_abs_valid and o2.ay_abs_valid:
                obj.ay_abs_valid = True
                obj.ay_abs = o1.ay_abs * w1 + o2.ay_abs * w2
            if o1.ax_rel_valid and o2.ax_rel_valid:
                obj.ax_rel_valid = True
                obj.ax_rel = o1.ax_rel * w1 + o2.ax_rel * w2
            if o1.ay_rel_valid and o2.ay_rel_valid:
                obj.ay_rel_valid = True
                obj.ay_rel = o1.ay_rel * w1 + o2.ay_rel * w2
            if o1.heading_valid and o2.heading_valid:
                obj.heading_valid = True
                obj.heading = _interpolate_angle(o1.heading, w1, o2.heading, w2)
            if o1.width_valid and o2.width_valid:
                obj.width_valid = True
                obj.width = o1.width * w1 + o2.width * w2
            if o1.length_valid and o2.length_valid:
                obj.length_valid = True
                obj.length = o1.length * w1 + o2.length * w2
            if o1.height_valid and o2.height_valid:
                obj.height_valid = True
                obj.height = o1.height * w1 + o2.height * w2
            if o1.yaw_rate_valid and o2.yaw_rate_valid:
                obj.yaw_rate_valid = True
                obj.yaw_rate = o1.yaw_rate * w1 + o2.yaw_rate * w2
            if o1.curvature_valid and o2.curvature_valid:
                obj.curvature_valid = True
                obj.curvature = o1.curvature * w1 + o2.curvature * w2
            obj.contour = o1.contour if w1 > w2 else o2.contour
            output.objects.append(obj)
            index += 1
    return output


# 获取ObjectSensorSample，用于样本输入
def get_obj_sensor_sample(channel):
    s1 = None
    s2 = None
    w1 = 0.0
    w2 = 0.0
    protocol_id_v5 = 'obj-sensor-sample-v5@' + str(channel)
    protocol_id_v6 = 'obj-sensor-sample-v6@' + str(channel)
    if protocol_id_v6 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v6]
        s1 = _conv_obj_sensor_sample_v6(pair.sample1)
        w1 = pair.weight1
        s2 = _conv_obj_sensor_sample_v6(pair.sample2)
        w2 = pair.weight2
    elif protocol_id_v5 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v5]
        s1 = _conv_obj_sensor_sample_v5(pair.sample1)
        w1 = pair.weight1
        s2 = _conv_obj_sensor_sample_v5(pair.sample2)
        w2 = pair.weight2
    if s1 is not None and s2 is not None:
        return _interpolate_obj_sensor_sample(s1, w1, s2, w2)
    return None
